%Author:Xiyin,li
%Date  :2021-07-12
%Copyright 2021 Xiyin,li.

%% Introfuction
%Calculate the torqe between permanent magnet
%By Dipole Model and Magnet Moment
%       |z
%       |
%       |_________y
%      /
%   x/
clc,clear
close all
%% STEP1:Set Parameters
Br = 1.465;
nu0 = 4*pi*10^(-7);
effector.radius = 5e-3;
effector.diameter = 2*effector.radius;
effector.height = 10e-3;
effector.volume = pi*(effector.radius^2)*effector.height;
effector.magDirection = [0;0;1];
effector.postion = [0;0;0];
effector.attitude0 = [0;0;0];


driver.radius = 5e-3;
driver.diameter = 2*driver.radius;
driver.height = 10e-3;
driver.volume = pi*(driver.radius^2)*driver.height;
driver.magDirection = [0;0;1];
driver.position = [0;100e-3;0];
driver.attitude0 = [0;0;0];

%% STEP2:Calculate Magnetic Moment[01] in World coordinate system.
effector.magMoment = effector.magDirection*(Br*effector.volume)/nu0;
  driver.magMoment =   driver.magDirection*(Br*  driver.volume)/nu0;

%% STEP3:Calculate Magnetic induction.
effector.magInduction.Ana = PMCir3D(Br,effector.diameter,effector.height,driver.position);
effector.magInduction.Dip = PMDipCir(effector.radius,  effector.height,driver.position,'z');


%% STEP4:Calculate Torqe[02].
Torqe.effector2driver = [];
Torqe.driver2effector = [];
for theta = linspace(0,pi*2,360)
%for theta = pi
    driver.attitude = [theta;0;0]+driver.attitude0;
    driver.rotationMatrix = Rot3(driver.attitude);
    effector.attitude = [0;0;0]+effector.attitude0;
    effector.rotationMatrix = Rot3(effector.attitude);
    
    driver.magInduction.Ana = PMCir3D(Br,driver.diameter,driver.height,-driver.rotationMatrix*driver.position);
    driver.magInduction.Dip = PMDipCir(driver.radius,driver.height,-driver.rotationMatrix*driver.position,'z');
    Torqe.effector2driver = [Torqe.effector2driver cross(driver.rotationMatrix*driver.magMoment,effector.magInduction.Dip)];
    Torqe.driver2effector = [Torqe.driver2effector cross(effector.magMoment,driver.rotationMatrix'*driver.magInduction.Dip)];
end

%% STEP5:Debug & Plot
figure(1)
hold on;grid on
set(gca,'FontSize',20,'Fontname', 'Times New Roman');
plot(linspace(0,360,360),Torqe.effector2driver(1,:),'b-','lineWidth',2);
plot(linspace(0,360,360),Torqe.driver2effector(1,:),'r-.','lineWidth',2);
Torqe.effector2driver
Torqe.driver2effector
%% Summary
%% Reference
%[01] https://en.wikipedia.org/wiki/Magnetic_moment
%[02] http://www.phys.ufl.edu/~acosta/phy2061/lectures/MagneticDipoles.pdf

%% Libary
function R = Rot3(attitude)
ax = attitude(1);
by = attitude(2);
cz = attitude(3);
Rz = [cos(cz) -sin(cz) 0;sin(cz) cos(cz) 0;0 0 1];
Ry = [cos(by) 0 sin(by);0 1 0;-sin(by) 0 cos(by)];
Rx = [1 0 0;0 cos(ax) -sin(ax);0 sin(ax) cos(ax)];
R = Rz*Ry*Rx;
end